#ifndef ROS_UTILS_TRANSFORM_HANDLER_H
#define ROS_UTILS_TRANSFORM_HANDLER_H

#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/buffer.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <geometry_msgs/msg/transform.hpp>
#include <memory>
#include <string>

namespace utils
{
class TransformHandler
{
public:
  explicit TransformHandler(const std::shared_ptr<rclcpp::Node>& node)
  : node_(node),
    tf_buffer_(std::make_unique<tf2_ros::Buffer>(node->get_clock())),
    tf_listener_(*tf_buffer_),
    tf_broadcaster_(node),
    static_tf_broadcaster_(node)
  {
  }

  std::pair<bool, geometry_msgs::msg::TransformStamped> getTransform(
    const std::string& target_frame,
    const std::string& source_frame,
    const rclcpp::Time& time,
    const rclcpp::Duration& timeout)
  {
    try {
      auto transform_stamped = tf_buffer_->lookupTransform(target_frame, source_frame, time, timeout);
      return std::make_pair(true, transform_stamped);
    }
    catch (const tf2::TransformException& ex) {
      RCLCPP_WARN_THROTTLE(
        node_->get_logger(),
        *(node_->get_clock()),
        1000,  // milliseconds
        "Failed to look up transform from %s to %s: %s",
        source_frame.c_str(), target_frame.c_str(), ex.what());
      return std::make_pair(false, geometry_msgs::msg::TransformStamped());
    }
  }

  std::pair<bool, geometry_msgs::msg::TransformStamped> getTransform(
    const std::string& source_frame,
    const std::string& target_frame)
  {
    return getTransform(source_frame, target_frame, rclcpp::Time(0), rclcpp::Duration(0, 100000000));  // 0.1 seconds
  }

  std::pair<bool, geometry_msgs::msg::TransformStamped> getTransform(
    const std::string& source_frame,
    const std::string& target_frame,
    const rclcpp::Time& time)
  {
    return getTransform(source_frame, target_frame, time, rclcpp::Duration(0, 100000000));  // 0.1 seconds
  }

  void sendTransform(
    const geometry_msgs::msg::Transform& transform,
    const std::string& target_frame,
    const std::string& source_frame,
    const rclcpp::Time& time)
  {
    geometry_msgs::msg::TransformStamped tf;
    tf.header.stamp = time;
    tf.header.frame_id = target_frame;
    tf.child_frame_id = source_frame;
    tf.transform = transform;
    tf_broadcaster_.sendTransform(tf);
  }

  void sendTransform(const geometry_msgs::msg::TransformStamped& transform_stamped)
  {
    tf_broadcaster_.sendTransform(transform_stamped);
  }

  void sendStaticTransform(
    const geometry_msgs::msg::Transform& transform,
    const std::string& target_frame,
    const std::string& source_frame)
  {
    geometry_msgs::msg::TransformStamped static_tf;
    static_tf.header.stamp = node_->get_clock()->now();
    static_tf.header.frame_id = target_frame;
    static_tf.child_frame_id = source_frame;
    static_tf.transform = transform;
    static_tf_broadcaster_.sendTransform(static_tf);
  }

  void sendStaticTransform(const geometry_msgs::msg::TransformStamped& transform_stamped)
  {
    static_tf_broadcaster_.sendTransform(transform_stamped);
  }

private:
  std::shared_ptr<rclcpp::Node> node_;
  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
  tf2_ros::TransformListener tf_listener_;
  tf2_ros::TransformBroadcaster tf_broadcaster_;
  tf2_ros::StaticTransformBroadcaster static_tf_broadcaster_;
};

}  // namespace utils

#endif  // ROS_UTILS_TRANSFORM_HANDLER_H